#include "message_queue.h"
#include <stdlib.h>
#include <string.h>
#include <threads.h>

#ifdef __cplusplus
extern "C" {
#endif

#define NPOS SIZE_MAX

struct cutils_mq {
    mtx_t mtx;
    cnd_t rcnd;
    cnd_t wcnd;

    size_t capacity;
    size_t max_size;
    size_t pause_size;
    size_t rindex;
    size_t windex;
    void *data[];
};

static size_t cutils_clp2(size_t x)
{
    x = x - 1u;
    x = x | (x >> 1u);
    x = x | (x >> 2u);
    x = x | (x >> 4u);
    x = x | (x >> 8u);
    x = x | (x >> 16u);
    if (sizeof(x) == 8) {
        x = x | (x >> 32u);
    }
    return x + 1u;
}

static bool cutils_mq_init(cutils_mq_t *mq, size_t capacity, size_t max_size)
{
    mq->capacity = capacity;
    mq->max_size = max_size;
    mq->pause_size = max_size / 2;
    mq->rindex = 0;
    mq->rindex = 0;

    if (mtx_init(&mq->mtx, mtx_timed) != thrd_success) {
        return false;
    }

    if (cnd_init(&mq->rcnd) != thrd_success) {
        return false;
    }

    if (cnd_init(&mq->wcnd) != thrd_success) {
        return false;
    }

    return true;
}

static void cutils_mq_exit(cutils_mq_t *mq)
{
    cnd_destroy(&mq->rcnd);
    cnd_destroy(&mq->wcnd);
    mtx_destroy(&mq->mtx);
}

static size_t cutils_mq_next(size_t capacity, size_t *index)
{
    size_t next = (*index)++;
    return next & (capacity - 1);
}

static size_t cutils_mq_next_rindex(cutils_mq_t *mq)
{
    if (cutils_mq_empty(mq)) {
        return NPOS;
    }

    return cutils_mq_next(mq->capacity, &mq->rindex);
}

static size_t cutils_mq_next_windex(cutils_mq_t *mq)
{
    if (cutils_mq_full(mq)) {
        return NPOS;
    }

    return cutils_mq_next(mq->capacity, &mq->windex);
}

static size_t cutils_mq_pause(cutils_mq_t *mq)
{
    return cutils_mq_size(mq) > mq->pause_size;
}

cutils_mq_t *cutils_mq_create(const cutils_mq_config_t *config)
{
    size_t capacity = cutils_clp2(config->queue_size);
    size_t msize = sizeof(cutils_mq_t) + capacity * sizeof(void *);
    cutils_mq_t *mq = (cutils_mq_t *)malloc(msize);
    if (mq == NULL) {
        return NULL;
    }

    (void)memset(mq, msize, 0);

    if (!cutils_mq_init(mq, capacity, config->queue_size)) {
        cutils_mq_destroy(mq);
        return NULL;
    }

    return mq;
}

void cutils_mq_destroy(cutils_mq_t *self)
{
    cutils_mq_exit(self);
    free(self);
}

bool cutils_mq_try_push(cutils_mq_t *self, void *mesg)
{
    size_t k = cutils_mq_next_windex(self);
    if (k == NPOS) {
        return false;
    }
    self->data[k] = mesg;
    return true;
}

bool cutils_mq_try_pop(cutils_mq_t *self, void **mesg)
{
    size_t k = cutils_mq_next_windex(self);
    if (k == NPOS) {
        return false;
    }
    *mesg = self->data[k];
    self->data[k] = NULL;
    return true;
}

int cutils_mq_push(cutils_mq_t *self, void *mesg, timespan_t timeout)
{
    if (cutils_mq_pause(self)) {
        return MQ_EPAUSE;
    }

    return MQ_EOK;
}

int cutils_mq_pop(cutils_mq_t *self, void **mesg, timespan_t timeout)
{

}

size_t cutils_mq_size(const cutils_mq_t *self)
{
    return self->windex - self->rindex;
}

size_t cutils_mq_capacity(const cutils_mq_t *self)
{
    return self->capacity;
}

bool cutils_mq_empty(const cutils_mq_t *self)
{
    return cutils_mq_size(self) == 0;
}

bool cutils_mq_full(const cutils_mq_t *self)
{
    return cutils_mq_size(self) == self->max_size;
}

#ifdef __cplusplus
}
#endif
